#include "ros/ros.h" //包含了使用ROS节点的必要文件

//visualization_msgs
#include "visualization_msgs/Marker.h"
#include "visualization_msgs/MarkerArray.h"
//uint8mutilarray
#include "std_msgs/UInt8MultiArray.h"
//pcl
#include <pcl/point_types.h>

#include <stdio.h>
#include <iostream>
#include <string>

ros::Publisher pub_uint8array;
ros::Subscriber sub_lidarBox;
double x_offset = 0.9679;
double y_offset = 0.0;
int frame_count = 5;
int frame_count_counter = 0;
bool fix_blank_data = false;
visualization_msgs::MarkerArray BoxMarkerArray_last_;

struct obstacle{
    uint8_t label;
    float confidence;
    std::vector<pcl::PointXYZ> points;
};

/**
     * @description: convert float to uint8array
     * @param {uint8_t} *u8Arry
     * @param {float} *floatdata
     * @param {bool} key:true Big-Endian; false-Little-Endian
     * @return {*}
     * @Author: Zhou Xiaohui
     * @Date: 2022-01-25 13:56:34
     */
void float2u8Arry(uint8_t *u8Arry, float *floatdata, bool key) //big small
{
    uint8_t farray[4];
    *(float *)farray = *floatdata;
    if (key == true)
    {
        u8Arry[3] = farray[0];
        u8Arry[2] = farray[1];
        u8Arry[1] = farray[2];
        u8Arry[0] = farray[3];
    }
    else
    {
        u8Arry[0] = farray[0];
        u8Arry[1] = farray[1];
        u8Arry[2] = farray[2];
        u8Arry[3] = farray[3];
    }
}

/**
     * @description: convert int16 to uint8array
     * @param {uint8_t} *u8Arry
     * @param {int16_t} *intdata
     * @param {bool} key:true Big-Endian; false-Little-Endian
     * @return {*}
     * @Author:
     * @Date: 2022-01-25 13:59:48
     */


void int162u8Arry(uint8_t *u8Arry, int16_t *intdata, bool key) //big small
{
    uint8_t farray[2];
    //方法一
//     uint16_t temp = *intdata;
    //方法二
    uint16_t temp ;
    if (*intdata >= 0)
    {
        temp = *intdata;
    }
    else
    {
        temp = 65535+*intdata +1;
    }
    farray[0] = temp & 0xFF;
    farray[1] = (temp >> 8) & 0xFF;
    if (key)
    {
        u8Arry[0] = farray[1];
        u8Arry[1] = farray[0];
    }
    else
    {
        u8Arry[0] = farray[0];
        u8Arry[1] = farray[1];
    }
}

/**
     * @description: convert uint16 to uint8array
     * @param {uint8_t} *u8Arry
     * @param {uint16_t} *intdata
     * @param {bool} key
     * @return {*}
     * @Author: Zhou Xiaohui
     * @Date: 2022-01-25 14:30:26
     */
void uint162u8Arry(uint8_t *u8Arry, uint16_t *intdata, bool key) //big small
{
    uint8_t farray[2];
    uint16_t temp = *intdata;
    //convert uint16_t to uint8_t, using little endian
    farray[0] = temp & 0xFF;
    farray[1] = (temp >> 8) & 0xFF;
    if (key)
    {
        u8Arry[0] = farray[1];
        u8Arry[1] = farray[0];
    }
    else
    {
        u8Arry[0] = farray[0];
        u8Arry[1] = farray[1];
    }
}

void lidarBoxCallback(const visualization_msgs::MarkerArrayConstPtr& BoxMarkerArrayPtr){
    visualization_msgs::MarkerArray BoxMarkerArray= *BoxMarkerArrayPtr;
    int boxSize = BoxMarkerArray.markers.size();

    if (fix_blank_data){
        if (boxSize > 0)
        {
            BoxMarkerArray_last_ = BoxMarkerArray;
            frame_count_counter = 0;
        } else{
            std::cout << "no box" << std::endl;
            if (frame_count_counter < frame_count)
            {
                BoxMarkerArray = BoxMarkerArray_last_;
                frame_count_counter++;
                std::cout << "updated box" << std::endl;
            }
            else
            {
                BoxMarkerArray_last_ = BoxMarkerArray;
                frame_count_counter = 0;
                std::cout << "reset" << std::endl;
            }
        }
    }

    std::vector<obstacle> obstacle_vec;
    obstacle_vec.clear();
    std::cout << "....................." << std::endl;

    for (size_t i = 0; i < BoxMarkerArray.markers.size(); i++)
    {
        obstacle mObstacle;
        mObstacle.confidence = 1.0;
        mObstacle.label = 1;

        float scalex = BoxMarkerArray.markers[i].scale.x /2;
        float scaley = BoxMarkerArray.markers[i].scale.y / 2;
        if ((scalex < 0.01) && (scaley < 0.01))
        {
            continue;
        }
        if ((BoxMarkerArray.markers[i].pose.position.x < 0.1) && (BoxMarkerArray.markers[i].pose.position.y < 0.1))
        {
            continue;
        }

        //* printf pose and scale
        std::cout << "雷达坐标系(单位m) 障碍物 " << i <<" 重心坐标: x(右): " << BoxMarkerArray.markers[i].pose.position.x << " y(前):" << BoxMarkerArray.markers[i].pose.position.y << " x方向长度:" << scalex*2 << " y方向长度:" << scaley*2;
        for (int signy = -1; signy <=1; signy += 2){
            for (int signx = -1; signx <=1 ;  signx += 2)
            {
                /* code */
                pcl::PointXYZ tmpPointXYZ;
                tmpPointXYZ.x = BoxMarkerArray.markers[i].pose.position.x + signx *scalex;
                tmpPointXYZ.y = BoxMarkerArray.markers[i].pose.position.y + signy *scaley;
                tmpPointXYZ.z = BoxMarkerArray.markers[i].pose.position.z;
                //* 打印角点
                std::cout << " -- 角点x:" << tmpPointXYZ.x << " y:" << tmpPointXYZ.y;
                mObstacle.points.push_back(tmpPointXYZ);
            }
        }
        std::cout << "\n------"<<std::endl;
        obstacle_vec.push_back(mObstacle);
    }
    //publish 2d lidar box
    /////////////////
    //length all-uint16->uint8,两位，后面更新值
    std_msgs::UInt8MultiArray r_buffer;
    r_buffer.data.push_back(0x01);
    r_buffer.data.push_back(0x01);

    //num of obstacle
    uint8_t mNum_obstacle_all = obstacle_vec.size() ;
    r_buffer.data.push_back(mNum_obstacle_all);

    for (size_t i_mcbv = 0; i_mcbv < obstacle_vec.size(); i_mcbv++)
    {
        //length i_mcbv
        uint16_t length_i_mcbv = obstacle_vec[i_mcbv].points.size() * 4 + 5; //xy csid value
        uint8_t length_i_mcbv_uint8[2];
        uint162u8Arry(length_i_mcbv_uint8, &length_i_mcbv, 0);
        r_buffer.data.push_back(length_i_mcbv_uint8[0]);
        r_buffer.data.push_back(length_i_mcbv_uint8[1]);

        //class id
        uint8_t clsid = obstacle_vec[i_mcbv].label;
        r_buffer.data.push_back(clsid);

        //convert confidence float to int16_t
        float confidence = obstacle_vec[i_mcbv].confidence;
        uint8_t confidence_uint8[4];
        float2u8Arry(confidence_uint8, &confidence, 0);
        r_buffer.data.push_back(confidence_uint8[0]);
        r_buffer.data.push_back(confidence_uint8[1]);
        r_buffer.data.push_back(confidence_uint8[2]);
        r_buffer.data.push_back(confidence_uint8[3]);

        //points
        for (int ip = 0; ip < obstacle_vec[i_mcbv].points.size(); ip++){
            float x = obstacle_vec[i_mcbv].points[ip].x;
            float y = obstacle_vec[i_mcbv].points[ip].y;
            {
                //transform to base_link
                float tmp =x;
                x= y + x_offset;
                y = -tmp + y_offset;
            }

            std::cout << "割草机坐标系(单位m) 障碍物 " << i_mcbv << " 角点 "<< ip <<" 坐标x(前):" << x << " y(左):" << y << std::endl;

            int16_t x0_2_int16 = (int)(x * 1000); //1000以内只占两个字节，所以强转？  change size unit from m -> mm
            uint8_t x0_2_uint8[2];
            int162u8Arry(x0_2_uint8, &x0_2_int16, 0);
            r_buffer.data.push_back(x0_2_uint8[0]);
            r_buffer.data.push_back(x0_2_uint8[1]);

            //change m to mm
            int16_t y0_2_int16 = (int)(y *1000 ) ; //1000以内只占两个字节，所以强转？
            uint8_t y0_2_uint8[2];
            int162u8Arry(y0_2_uint8, &y0_2_int16, 0);
            r_buffer.data.push_back(y0_2_uint8[0]);
            r_buffer.data.push_back(y0_2_uint8[1]);
        }
        std::cout << "------"<<std::endl;
    }

    uint16_t length_all = r_buffer.data.size()-2;
    uint8_t length_all_2_uint8[2];
    uint162u8Arry(length_all_2_uint8, &length_all, 0);
    r_buffer.data[0] = length_all_2_uint8[0];
    r_buffer.data[1] = length_all_2_uint8[1];
    ROS_INFO("whole length: %u", length_all);
    ROS_INFO("little: %u", length_all & 0xFF);
    ROS_INFO("big: %u", (length_all >> 8) & 0xFF);
    //printf
    for (size_t i = 0; i < r_buffer.data.size(); i++)
    {
        printf("%02x ", r_buffer.data[i]);
    }
    printf("\n");
    for (size_t i = 0; i < r_buffer.data.size(); i++)
    {
        printf("%d ", r_buffer.data[i]);
    }
    pub_uint8array.publish(r_buffer);
    std::cout << "....................." << std::endl;
}

int main(int argc, char **argv)
{
    ///////////////////////////////////////////////////////////////////////////////
    ////////////////////////////////ROS
    ros::init(argc, argv, "sub_lidarbox_node");                                      //初始化ROS，节点名命名为node_a，节点名必须保持唯一
    ros::NodeHandle n;                                                    //实例化节点, 节点进程句柄
    ros::Rate loop_rate(30);                                              //设置发送数据的频率为10Hz

    //read para from launch
    n.getParam("x_offset", x_offset);
    n.getParam("y_offset", y_offset);
    n.getParam("frame_count", frame_count);
    n.getParam("fix_blank_data", fix_blank_data);
    ROS_INFO("x_offset parameter is................... %f", x_offset);
    ROS_INFO("y_offset parameter is................... %f", y_offset);
    ROS_INFO("frame_count parameter is................... %d", frame_count);
    ROS_INFO("fix_blank_data parameter is................... %d", fix_blank_data);

    sub_lidarBox = n.subscribe("/objseg_gridclass/LidarObjBox", 1, lidarBoxCallback);
    pub_uint8array = n.advertise<std_msgs::UInt8MultiArray>("/cloud_point_ai", 1);
    while (ros::ok())
    {
        ros::spinOnce();   //不是必须，若程序中订阅话题则必须，否则回掉函数不起作用。
        loop_rate.sleep(); //按前面设置的10Hz频率将程序挂起
    }
    return 0;
}
